#include <stdio.h>
#include "vl53l0x_platform.h"
#include "vl53l0x_api.h"

#include "SoftIIC.h"

#include "OpticalCar_hardware.h"


static SoftIIC_HardwareTypeDef g_iic_hw_1, g_iic_hw_2;
static VL53L0X_Dev_t g_dev1, g_dev2;


static void VL53L0X_HardwareInit(void) {
    // ===== Config IIC hardware =====
    g_iic_hw_1.RCC_SCL = RCC_SCL1;
    g_iic_hw_1.RCC_SDA = RCC_SDA1;
    g_iic_hw_1.port_SCL = PORT_IIC_SCL1;
    g_iic_hw_1.pin_SCL = PIN_IIC_SCL1;
    g_iic_hw_1.port_SDA = PORT_IIC_SDA1;
    g_iic_hw_1.pin_SDA = PIN_IIC_SDA1;
    
    SoftIIC_Init(&g_iic_hw_1);
    
    g_iic_hw_2.RCC_SCL = RCC_SCL2;
    g_iic_hw_2.RCC_SDA = RCC_SDA2;
    g_iic_hw_2.port_SCL = PORT_IIC_SCL2;
    g_iic_hw_2.pin_SCL = PIN_IIC_SCL2;
    g_iic_hw_2.port_SDA = PORT_IIC_SDA2;
    g_iic_hw_2.pin_SDA = PIN_IIC_SDA2;
    
    SoftIIC_Init(&g_iic_hw_2);
    
    g_dev1.iic_addr = 0x52;
    g_dev2.iic_addr = 0x52;
    g_dev1.iic_hw = &g_iic_hw_1;
    g_dev2.iic_hw = &g_iic_hw_2;
}


static void VL53L0X_Init(void) {
    VL53L0X_Error _err;
    uint32_t refSpadCount;
    uint8_t isApertureSpads;
    uint8_t VhvSettings;
    uint8_t PhaseCal;
    
    // ===== I2C hardware init =====
    VL53L0X_HardwareInit();
    
    // ===== device init =====
    _err = VL53L0X_DataInit(&g_dev1);
    printf("d init:%i,", _err);
    
    _err = VL53L0X_StaticInit(&g_dev1);
    printf("s init:%i,", _err);
    
    // ===== calibration =====
    _err = VL53L0X_PerformRefCalibration(&g_dev1, &VhvSettings, &PhaseCal); // Device Initialization
    printf("ref:%i,",_err);
    
    _err = VL53L0X_PerformRefSpadManagement(&g_dev1, &refSpadCount, &isApertureSpads); // Device Initialization
    printf("pad:%i,",_err);
    
    // ===== dev mode =====
    //_err = VL53L0X_SetDeviceMode(&g_dev1, VL53L0X_DEVICEMODE_SINGLE_RANGING);
    printf("set mod:%i,", _err);
    
    // skip GPIO (polling and interrput) config
}


static void Test_SingleRanging(void) {
    
    VL53L0X_Error Status = VL53L0X_ERROR_NONE;
    VL53L0X_RangingMeasurementData_t _meas;
    

    // ===== copied from Single Ranging Example ===== 
//    if (Status == VL53L0X_ERROR_NONE) {
//        Status = VL53L0X_SetLimitCheckEnable(&g_dev1,
//        VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
//        printf("%i,",Status);
//    }
//    if (Status == VL53L0X_ERROR_NONE) {
//        Status = VL53L0X_SetLimitCheckEnable(&g_dev1,
//        VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
//        printf("%i,",Status);
//    }

//    if (Status == VL53L0X_ERROR_NONE) {
//        Status = VL53L0X_SetLimitCheckEnable(&g_dev1,
//        VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 1);
//        printf("%i,",Status);
//    }

//    if (Status == VL53L0X_ERROR_NONE) {
//        Status = VL53L0X_SetLimitCheckValue(&g_dev1,
//        VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
//        (FixPoint1616_t)(1.5*0.023*65536));
//        printf("%i,",Status);
//    }
    
    
    // ===== perform measurement =====
    Status = VL53L0X_PerformSingleRangingMeasurement(&g_dev1, &_meas);
    //printf("m:%i,", _err);
    
    // _err = VL53L0X_GetRangingMeasumentData(&g_dev1, &_meas);
    printf("%3i,%4i,%3i    ",
        Status, _meas.RangeMilliMeter, _meas.RangeStatus);
}


static void VL53L0X_Test(void) {
    VL53L0X_Init();
    for (int i=0; i<5; i++) {
        delay_us(1000*1000);
    }
    
    printf("\x7f");
    printf("start meas...   ");
    while (1) {
        Test_SingleRanging();
        //delay_us(1000*1000);
    }
}


static void VL53L0X_2_Init(void) {
    // ===== I2C hardware init =====
    VL53L0X_HardwareInit();
    
    // ===== Dev Init =====
    VL53L0X_Error _err1, _err2;
    uint32_t refSpadCount;
    uint8_t isApertureSpads;
    uint8_t VhvSettings;
    uint8_t PhaseCal;
    
    _err1 = VL53L0X_DataInit(&g_dev1);
    _err2 = VL53L0X_DataInit(&g_dev2);
    printf("d init:%i,%i", _err2, _err1);
    
    _err1 = VL53L0X_StaticInit(&g_dev1);
    _err2 = VL53L0X_StaticInit(&g_dev2);
    printf("s init:%i,%i", _err2, _err1);
    
    // ===== Calibration =====
    _err1 = VL53L0X_PerformRefCalibration(&g_dev1, &VhvSettings, &PhaseCal); // Device Initialization
    _err2 = VL53L0X_PerformRefCalibration(&g_dev2, &VhvSettings, &PhaseCal); // Device Initialization
    printf("ref:%i,%i",_err2,_err1);
    
    _err1 = VL53L0X_PerformRefSpadManagement(&g_dev1, &refSpadCount, &isApertureSpads); // Device Initialization
    _err2 = VL53L0X_PerformRefSpadManagement(&g_dev2, &refSpadCount, &isApertureSpads); // Device Initialization
    printf("pad:%i,%i",_err2,_err1);
}


static void VL53L0X_2_Test(void) {
    VL53L0X_2_Init();
    
    VL53L0X_RangingMeasurementData_t _meas1, _meas2;
    printf("\x7f");
    while (1) {
        VL53L0X_PerformSingleRangingMeasurement(&g_dev1, &_meas1);
        VL53L0X_PerformSingleRangingMeasurement(&g_dev2, &_meas2);
        
        printf("\x7f%4i %4i %6s", _meas2.RangeMilliMeter, _meas1.RangeMilliMeter, "");
        
        //printf("  %4i    %4i  ", _meas2.RangeStatus, _meas1.RangeStatus);
    }
}
